/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.defaultCode;

import java.util.TimerTask;
import edu.wpi.first.wpilibj.Watchdog;
import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.PIDController;



/**
 *
 * @author beto
 *
 */
public class DriveTrain extends TimerTask
{

    // ----------------------------------------
    // Create Structures
    private WheelSet Current = new WheelSet();
    private WheelSet Target = new WheelSet();
    private WheelSet Next = new WheelSet();
    private ResultantVector rMaxStepSize = new ResultantVector();
    private WheelVector wMaxStepSize = new WheelVector();
    private ResultantVector goal = new ResultantVector();

    // ----------------------------------------
    // Create Working Variables
    // They should be local to Update(), but we create them as static
    // so they are not continuously created and destroyed (garbage collection)
    // WheelSet Working Variables
    private WheelSet Delta = new WheelSet();
    private WheelSet StepSize = new WheelSet();
    private WheelSet NumSteps = new WheelSet();
    private Joystick driverJoy;

    // ----------------------------------------
    // Motors and Encoders

    // Encoders
    REncoder flEnc = new REncoder(1,4, "FL");
    REncoder frEnc = new REncoder(1,1, "FR");
    REncoder rlEnc = new REncoder(1,3, "RL");
    REncoder rrEnc = new REncoder(1,2, "RR");

    // Motors that steer wheels
    private Jaguar flTurnMtr;
    private Jaguar frTurnMtr;
    private Jaguar rlTurnMtr;
    private Jaguar rrTurnMtr;
    private PIDController flPID;
    private PIDController frPID;
    private PIDController rlPID;
    private PIDController rrPID;

    // Motors that move wheels forward and back
    private Jaguar flDriveMtr;
    private Jaguar rlDriveMtr;
    private Jaguar frDriveMtr;
    private Jaguar rrDriveMtr;

    // For the PID Controllers
    private double p = .25;
    private double i = 0;
    private double d = 0;

    // Special Commands
    private boolean moveSteeringToZero;

    private boolean pauseDT = false;

    private IThroughBeamCallback beamSensor;
    private int autoKickerCounter = 0; //counts up to some number
    private int warmupCounter = 10; //This counts down to zero before starting
    // Misc
    Averager joyAngleAvg;
    DriveTrain(Joystick joy, IThroughBeamCallback sensor)
    {
                joyAngleAvg = new Averager();
                beamSensor = sensor;
        System.out.println("DT constructor called.");
        // Set Default Accelerations and Speeds
        // (as Increments in a 10msec Update Rate)
    
        rMaxStepSize.Speed = 0.5;
        rMaxStepSize.Angle = 2.0;
        rMaxStepSize.Spin = 0.5;

        wMaxStepSize.Speed = 0.5;
        wMaxStepSize.Angle = 2.0;


        // Initialize

        Current.Resultant.Speed = 0.0;
        Current.Resultant.Angle = 0.0;
        Current.Resultant.Spin = 0.0;

        Target.Resultant.Speed = 0.0;
        Target.Resultant.Angle = 0.0;
        Target.Resultant.Spin = 0.0;

        
        // Initialize Drives
        //
        // Note: that in the hardware the speed controllers, motors and wheels
        // are numbered from the front-right wheel, and clockwise, such that:
        //    front-right = 1 (for the drive motor) and 5 (for the steering motor)
        //    front-left  = 2 (for the drive motor) and 6 (for the steering motor)
        //    rear-left   = 3 (for the drive motor) and 7 (for the steering motor)
        //    rear-right  = 4 (for the drive motor) and 8 (for the steering motor)
        //
        // In the software, however, we always order them such that when looking
        // at the robot from above and the front is up, we read from left to right
        // and top to bottom, and therefore the order is:
        //    front-left
        //    front-right
        //    rear-left
        //    rear-right

        flTurnMtr = new Jaguar(4, 6);
        frTurnMtr = new Jaguar(4, 5);
        rlTurnMtr = new Jaguar(4, 7);
        rrTurnMtr = new Jaguar(4, 8);

        flDriveMtr = new Jaguar(4, 2);
        frDriveMtr = new Jaguar(4, 1);
        rlDriveMtr = new Jaguar(4, 3);
        rrDriveMtr = new Jaguar(4, 4);

        driverJoy = joy;

        // Initialize Special Commands
        moveSteeringToZero = false;

    }


    public void SetTargetResultant(ResultantVector InputTarget ){

        if( !moveSteeringToZero)
        {

            // ----------------------------------------
            // Set Target for the Resultant

            Target.Resultant.Speed = InputTarget.Speed;
            if( Math.abs(Target.Resultant.Speed) > 0 )
                Target.Resultant.Angle = InputTarget.Angle;
            Target.Resultant.Spin = InputTarget.Spin;

            Delta.Resultant.Angle = RotaryShortestPath(Target.Resultant.Angle, Current.Resultant.Angle);

            if( Math.abs(Delta.Resultant.Angle) > 90.0 &&
                ( Math.abs(Current.Resultant.Speed) > 10*Math.abs(Current.Resultant.Spin) || ( Math.abs(Current.Resultant.Speed) < 0.1 && Math.abs(Current.Resultant.Spin) < 0.01 ) ) )
            {
                Target.Resultant.Speed = -InputTarget.Speed;
                Target.Resultant.Angle = InputTarget.Angle + 180.0;
            }


            // ----------------------------------------
            // Set Target for each of the Wheels

            Target = CalculateWheelsFrontomResultant(Target.Resultant);

            Delta.FrontLeft.Angle = RotaryShortestPath(Target.FrontLeft.Angle, Current.FrontLeft.Angle);
            if( Math.abs(Delta.FrontLeft.Angle) > 90.0 ){
                Target.FrontLeft.Speed = -Target.FrontLeft.Speed;
                Target.FrontLeft.Angle = Target.FrontLeft.Angle + 180.0;
            }

            Delta.FrontRight.Angle = RotaryShortestPath(Target.FrontRight.Angle, Current.FrontRight.Angle);
            if( Math.abs(Delta.FrontRight.Angle) > 90.0 ){
                Target.FrontRight.Speed = -Target.FrontRight.Speed;
                Target.FrontRight.Angle = Target.FrontRight.Angle + 180.0;
            }

            Delta.RearLeft.Angle = RotaryShortestPath(Target.RearLeft.Angle, Current.RearLeft.Angle);
            if( Math.abs(Delta.RearLeft.Angle) > 90.0 ){
                Target.RearLeft.Speed = -Target.RearLeft.Speed;
                Target.RearLeft.Angle = Target.RearLeft.Angle + 180.0;
            }

            Delta.RearRight.Angle = RotaryShortestPath(Target.RearRight.Angle, Current.RearRight.Angle);
            if( Math.abs(Delta.RearRight.Angle) > 90.0 ){
                Target.RearRight.Speed = -Target.RearRight.Speed;
                Target.RearRight.Angle = Target.RearRight.Angle + 180.0;
            }
        }
        else
        {
            Target.Resultant.Speed = 0.0;
            Target.Resultant.Angle = 0.0;
            Target.Resultant.Spin = 0.0;

            Target.FrontLeft.Speed = 0.0;
            Target.FrontLeft.Angle = 0.0;

            Target.FrontRight.Speed = 0.0;
            Target.FrontRight.Angle = 0.0;

            Target.RearLeft.Speed = 0.0;
            Target.RearLeft.Angle = 0.0;

            Target.RearRight.Speed = 0.0;
            Target.RearRight.Angle = 0.0;

        }

    }


    public void Update( ){

        if( !moveSteeringToZero)
        {

            // ----------------------------------------
            // Calculate Next

            CalculateNextResultantFrontomCurrent();

            // ----------------------------------------
            // Calculate Next Wheels

            Next = CalculateWheelsFrontomResultant(Next.Resultant);

            // don't steer if we don't have to

            if( Math.abs(Next.FrontLeft.Speed) == 0 )
                Next.FrontLeft.Angle = Current.FrontLeft.Angle;

            if( Math.abs(Next.FrontRight.Speed) == 0 )
                Next.FrontRight.Angle = Current.FrontRight.Angle;

            if( Math.abs(Next.RearLeft.Speed) == 0 )
                Next.RearLeft.Angle = Current.RearLeft.Angle;

            if( Math.abs(Next.RearRight.Speed) == 0 )
                Next.RearRight.Angle = Current.RearRight.Angle;


            // which way?

            Delta.FrontLeft.Angle = RotaryShortestPath(Next.FrontLeft.Angle, Current.FrontLeft.Angle);
            if( Math.abs(Delta.FrontLeft.Angle) > 90.0 ){
                Next.FrontLeft.Speed = -Next.FrontLeft.Speed;
                Next.FrontLeft.Angle = Next.FrontLeft.Angle + 180.0;
            }

            Delta.FrontRight.Angle = RotaryShortestPath(Next.FrontRight.Angle, Current.FrontRight.Angle);
            if( Math.abs(Delta.FrontRight.Angle) > 90.0 ){
                Next.FrontRight.Speed = -Next.FrontRight.Speed;
                Next.FrontRight.Angle = Next.FrontRight.Angle + 180.0;
            }

            Delta.RearLeft.Angle = RotaryShortestPath(Next.RearLeft.Angle, Current.RearLeft.Angle);
            if( Math.abs(Delta.RearLeft.Angle) > 90.0 ){
                Next.RearLeft.Speed = -Next.RearLeft.Speed;
                Next.RearLeft.Angle = Next.RearLeft.Angle + 180.0;
            }

            Delta.RearRight.Angle = RotaryShortestPath(Next.RearRight.Angle, Current.RearRight.Angle);
            if( Math.abs(Delta.RearRight.Angle) > 90.0 ){
                Next.RearRight.Speed = -Next.RearRight.Speed;
                Next.RearRight.Angle = Next.RearRight.Angle + 180.0;
            }


            // ----------------------------------------
            // Update All

            Current.Resultant.Speed = Next.Resultant.Speed;
            Current.Resultant.Angle = Next.Resultant.Angle;
            Current.Resultant.Spin = Next.Resultant.Spin;

            Current.FrontLeft.Speed = Next.FrontLeft.Speed;
            Current.FrontLeft.Angle = Next.FrontLeft.Angle;

            Current.FrontRight.Speed = Next.FrontRight.Speed;
            Current.FrontRight.Angle = Next.FrontRight.Angle;

            Current.RearLeft.Speed = Next.RearLeft.Speed;
            Current.RearLeft.Angle = Next.RearLeft.Angle;

            Current.RearRight.Speed = Next.RearRight.Speed;
            Current.RearRight.Angle = Next.RearRight.Angle;
        }
        else
        {
            Current.Resultant.Speed = Next.Resultant.Speed = 0.0;
            Current.Resultant.Angle = Next.Resultant.Angle = 0.0;
            Current.Resultant.Spin = Next.Resultant.Spin = 0.0;

            Current.FrontLeft.Speed = Next.FrontLeft.Speed = 0.0;
            Current.FrontLeft.Angle = Next.FrontLeft.Angle = 0.0;

            Current.FrontRight.Speed = Next.FrontRight.Speed = 0.0;
            Current.FrontRight.Angle = Next.FrontRight.Angle = 0.0;

            Current.RearLeft.Speed = Next.RearLeft.Speed = 0.0;
            Current.RearLeft.Angle = Next.RearLeft.Angle = 0.0;

            Current.RearRight.Speed = Next.RearRight.Speed = 0.0;
            Current.RearRight.Angle = Next.RearRight.Angle = 0.0;
        }


        // ----------------------------------------
        // Output Values to the Drive Motors and to the Steering PID Controllers

        if(!pauseDT)
        {
        flPID.setSetpoint(Utils.mod360(Current.FrontLeft.Angle));
      //  System.out.println("FL target: " + Utils.mod360(Current.FrontLeft.Angle));
        frPID.setSetpoint(Utils.mod360(Current.FrontRight.Angle));
        rlPID.setSetpoint(Utils.mod360(Current.RearLeft.Angle));
        rrPID.setSetpoint(Utils.mod360(Current.RearRight.Angle));


        flDriveMtr.set(Current.FrontLeft.Speed);
        frDriveMtr.set(-Current.FrontRight.Speed);
        rlDriveMtr.set(Current.RearLeft.Speed);
        rrDriveMtr.set(-Current.RearRight.Speed);
        }


    }


    private void CalculateNextResultantFrontomCurrent(){

        // ----------------------------------------
        // Calculate Delta (Current to Target) for the Resultant

        Delta.Resultant.Speed = Target.Resultant.Speed - Current.Resultant.Speed;
        Delta.Resultant.Angle = RotaryShortestPath(Target.Resultant.Angle, Current.Resultant.Angle);
        Delta.Resultant.Spin = Target.Resultant.Spin - Current.Resultant.Spin;


        // ----------------------------------------
        // Calculate Number of Steps (Increments) to Target

        NumSteps.Resultant.Speed = Math.abs( Delta.Resultant.Speed / rMaxStepSize.Speed );
        NumSteps.Resultant.Angle = Math.abs( Delta.Resultant.Angle / rMaxStepSize.Angle );
        NumSteps.Resultant.Spin = Math.abs( Delta.Resultant.Spin / rMaxStepSize.Spin );

        // The number of steps to target will be the largest of the 3

        double rNumSteps = 0;
        if( NumSteps.Resultant.Speed > rNumSteps )
            rNumSteps = NumSteps.Resultant.Speed;
        if( NumSteps.Resultant.Angle > rNumSteps )
            rNumSteps = NumSteps.Resultant.Angle;
        if( NumSteps.Resultant.Spin > rNumSteps )
            rNumSteps = NumSteps.Resultant.Spin;


        // ----------------------------------------
        // Calculate Step Size for the Resultant

        StepSize.Resultant.Speed = Delta.Resultant.Speed / rNumSteps;
        StepSize.Resultant.Angle = Delta.Resultant.Angle / rNumSteps;
        StepSize.Resultant.Spin = Delta.Resultant.Spin / rNumSteps;


        // ----------------------------------------
        // Calculate Next Resultant
        // Converge to target when close enough

        if( Math.abs(Target.Resultant.Speed - Current.Resultant.Speed) < rMaxStepSize.Speed )
            Next.Resultant.Speed = Target.Resultant.Speed;
        else{
            Next.Resultant.Speed = Current.Resultant.Speed + StepSize.Resultant.Speed;
            if( Math.abs(Target.Resultant.Speed - Current.Resultant.Speed) < rMaxStepSize.Speed )
                Next.Resultant.Speed = Target.Resultant.Speed;
        }

        if( Math.abs( RotaryShortestPath( Target.Resultant.Angle, Current.Resultant.Angle ) ) < rMaxStepSize.Angle )
            Next.Resultant.Angle = Target.Resultant.Angle;
        else{
            Next.Resultant.Angle = Current.Resultant.Angle + StepSize.Resultant.Angle;
            if( Math.abs( RotaryShortestPath( Target.Resultant.Angle, Current.Resultant.Angle ) ) < rMaxStepSize.Angle )
                Next.Resultant.Angle = Target.Resultant.Angle;
        }

        if( Math.abs(Target.Resultant.Spin - Current.Resultant.Spin) < rMaxStepSize.Spin )
            Next.Resultant.Spin = Target.Resultant.Spin;
        else{
            Next.Resultant.Spin = Current.Resultant.Spin + StepSize.Resultant.Spin;
            if( Math.abs(Target.Resultant.Spin - Current.Resultant.Spin) < rMaxStepSize.Spin )
                Next.Resultant.Spin = Target.Resultant.Spin;
        }

    }


    private WheelSet CalculateWheelsFrontomResultant(ResultantVector Input){

        WheelSet Output = new WheelSet();

        double v = Output.Resultant.Speed = Input.Speed;
        double a = Output.Resultant.Angle = Input.Angle;
        double w = Output.Resultant.Spin = Input.Spin;

        double max;

        double vx;
        double vy;

        double FLvx;
        double FLvy;
        double FRvx;
        double FRvy;
        double RLvx;
        double RLvy;
        double RRvx;
        double RRvy;


        // ----------------------------------------
        // Enforce Ranges

        max = Math.max( Math.abs(v), Math.abs(w) );

        if( max > 1.0 ){
            v = v/max;
            w = w/max;
        }


        // ----------------------------------------
        // Transform to Cartesian

        vx = -v*Math.sin(Math.toRadians(a));
        vy = v*Math.cos(Math.toRadians(a));


        // ----------------------------------------
        // Calculate each WheelVector in Cartesian

        FLvx = vx - w;
        FLvy = vy - w;

        FRvx = vx - w;
        FRvy = vy + w;

        RLvx = vx + w;
        RLvy = vy - w;

        RRvx = vx + w;
        RRvy = vy + w;


        // ----------------------------------------
        // Transform Input to Polar

        Output.FrontLeft.Speed = Math.sqrt( FLvx*FLvx + FLvy*FLvy );
        Output.FrontLeft.Angle = Math.toDegrees(-MathUtils.atan2(FLvx, FLvy));

        Output.FrontRight.Speed = Math.sqrt( FRvx*FRvx + FRvy*FRvy );
        Output.FrontRight.Angle = Math.toDegrees(-MathUtils.atan2(FRvx, FRvy));

        Output.RearLeft.Speed = Math.sqrt( RLvx*RLvx + RLvy*RLvy );
        Output.RearLeft.Angle = Math.toDegrees(-MathUtils.atan2(RLvx, RLvy));

        Output.RearRight.Speed = Math.sqrt( RRvx*RRvx + RRvy*RRvy );
        Output.RearRight.Angle = Math.toDegrees(-MathUtils.atan2(RRvx, RRvy));


        // ----------------------------------------
        // Scale Wheels Speeds for Max = 1.0

        max = 0.0;

        if( Math.abs(Output.FrontLeft.Speed) > max )
            max = Math.abs(Output.FrontLeft.Speed);
        if( Math.abs(Output.FrontRight.Speed) > max )
            max = Math.abs(Output.FrontRight.Speed);
        if( Math.abs(Output.RearLeft.Speed) > max )
            max = Math.abs(Output.RearLeft.Speed);
        if( Math.abs(Output.RearRight.Speed) > max )
            max = Math.abs(Output.RearRight.Speed);

        if( max > 1.0 ){
            Output.FrontLeft.Speed = Output.FrontLeft.Speed / max;
            Output.FrontRight.Speed = Output.FrontRight.Speed / max;
            Output.RearLeft.Speed = Output.RearLeft.Speed / max;
            Output.RearRight.Speed = Output.RearRight.Speed / max;
        }

        return Output;

    }


    /**
     * RotaryShortestPath()
     *
     * Returns the shortest delta path from current angle to target angle.
     *
     */
    private double RotaryShortestPath( double Target, double Current ){

        double Output = Target - Current;

        Output = Output % 360.0;

        if( Output < -180.0 )
            Output = 360.0 + Output;
        if( Output > 180.0 )
            Output = Output - 360.0;

        return Output;

    }


    /**
     * NormalizeAngle()
     *
     *  Computes the normalized value of an angle, which is the angle
     *  expressed in the interval (-180.0, 180.0].
     *  We use this for printing.
     */
    private double NormalizeAngle(double Angle){

        Angle = Angle % 360.0;

        if( Angle > 180.0 )
            Angle = Angle = Angle - 360.0;
        if( Angle < -180.0 ){
            Angle = Angle = 360.0 + Angle;
        }
        if( Angle == -180.0 )
            Angle = 180.0;

        return Angle;

    }


    public void run()
    {
        Watchdog.getInstance().feed();

        if(pauseDT)
        {
            return;
        }

        if(DriverStation.getInstance().isAutonomous())
        {
            driveAuto();
        }
        else
        {
            driveTele();
        }

        // ----------------------------------------
        // Set Target that was updated in the driveTele/driveAuto methods

        SetTargetResultant(goal);

        // ----------------------------------------
        // Refresh

        Update();


        // ----------------------------------------
        // Print to Screen
        // Print only if button 2 pressed (otherwise too much clutter)
        
        if(driverJoy.getRawButton(2))
        {
            PrintWheelSet(Target);
            System.out.println("     -------------------------(t)");
            PrintCurrent();
        }
//
//        System.out.println("----------");
//        System.out.println("LF: " + flEnc.getRawValue());
//        System.out.println("RF: " + frEnc.getRawValue());
//        System.out.println("LR: " + rlEnc.getRawValue());
//        System.out.println("RR: " + rrEnc.getRawValue());

    }


    /**
     * Called after the teleoperated period ends. Sets the setpoints back to the
     * original zero position, then wait an appropriate amount of time before
     * disabling the PID.
     */
    public void shutDown()
    {
        Watchdog.getInstance().feed();
        flPID.setSetpoint(0);
        frPID.setSetpoint(0);
        rlPID.setSetpoint(0);
        rrPID.setSetpoint(0);
        try
        {
            Thread.sleep(3000);

        } catch (InterruptedException ex)
        {
            ex.printStackTrace();
        }

        flPID.disable();
        frPID.disable();
        rlPID.disable();
        rrPID.disable();
    }


    /**
     * Returns the squared value of the joystick value passed to it. Adds further
     * granularity in the values of the joystick.
     * @param val Value to square
     * @return Square of the parameter, muliplied by the original sign
     */
    private double getJoySquared(double val)
    {
        if(val < -1)
        {
            return (-1*val*val);
        }
        else
        {
            return val*val;
        }
    }


    public void initialize()
    {
        System.out.println("DT init called.");
        
        initializeEncoderAngles();
        
        initMaintenanceMode();

        setupPID();
    }


    private void setupPID()
    {
        flPID = new PIDController(p, i, d, flEnc, flTurnMtr, .005);
        frPID = new PIDController(p, i, d, frEnc, frTurnMtr, .005);
        rlPID = new PIDController(p, i, d, rlEnc, rlTurnMtr, .005);
        rrPID = new PIDController(p, i, d, rrEnc, rrTurnMtr, .005);
        
        flPID.setTolerance(.3);
        frPID.setTolerance(.3);
        rlPID.setTolerance(.3);
        rrPID.setTolerance(.3);

        flPID.setInputRange(0, 360);
        frPID.setInputRange(0, 360);
        rlPID.setInputRange(0, 360);
        rrPID.setInputRange(0, 360);

        flPID.setContinuous();
        frPID.setContinuous();
        rlPID.setContinuous();
        rrPID.setContinuous();
        
        flPID.enable();
        frPID.enable();
        rlPID.enable();
        rrPID.enable();
    }


    private void initializeEncoderAngles()
    {
        flEnc.zeroAngle();
        frEnc.zeroAngle();
        rlEnc.zeroAngle();
        rrEnc.zeroAngle();
    }


    public void PrintResultant(ResultantVector Input){

        String s = "";

        s = s + Utils.StringFormat63f( Input.Speed );
        s = s + "  ";
        s = s + Utils.StringFormat61f( NormalizeAngle(Input.Angle) );
        s = s + "  ";
        s = s + Utils.StringFormat63f( Input.Spin );

        System.out.println(s);

    }


    private void PrintWheelSet(WheelSet Input){

        String s = "";

//        s = s + Utils.StringFormat63f( Input.Resultant.Speed);
//        s = s + "  ";
//        s = s + Utils.StringFormat61f( NormalizeAngle(Input.Resultant.Angle));
//        s = s + "  ";
//        s = s + Utils.StringFormat63f( Input.Resultant.Spin);

  //      s = s + "        ";

        s = s + Utils.StringFormat63f( Input.FrontLeft.Speed);
        s = s + "  Target angle: ";
        s = s + Utils.StringFormat61f( NormalizeAngle(Input.FrontLeft.Angle));

//        s = s + "    ";
//
//        s = s + Utils.StringFormat63f( Input.FrontRight.Speed);
//        s = s + "  ";
//        s = s + Utils.StringFormat61f( NormalizeAngle(Input.FrontRight.Angle));
//
//        s = s + "    ";
//
//        s = s + Utils.StringFormat63f( Input.RearLeft.Speed);
//        s = s + "  ";
//        s = s + Utils.StringFormat61f( NormalizeAngle(Input.RearLeft.Angle));
//
//        s = s + "    ";
//
//        s = s + Utils.StringFormat63f( Input.RearRight.Speed);
//        s = s + "  ";
//        s = s + Utils.StringFormat61f( NormalizeAngle(Input.RearRight.Angle));

        System.out.println(s);

    }


    public void PrintCurrent(){

        PrintWheelSet( Current );

        String s = "";

//        s = s + "      ";
//        s = s + "  ";
//        s = s + "      ";
//        s = s + "  ";
//        s = s + "      ";
//
//        s = s + "        ";
//
//        s = s + "      ";
//        s = s + "  ";
        s = s + "LF curr angle: " + Utils.StringFormat61f( NormalizeAngle(flEnc.getAngle()));

//        s = s + "    ";
//
//        s = s + "      ";
//        s = s + "  ";
//    //    s = s + Utils.StringFormat61f( NormalizeAngle(frEnc.getAngle()));
//
//        s = s + "    ";
//
//        s = s + "      ";
//        s = s + "  ";
//      //  s = s + Utils.StringFormat61f( NormalizeAngle(rlEnc.getAngle()));
//
//        s = s + "    ";
//
//        s = s + "      ";
//        s = s + "  ";
        //s = s + Utils.StringFormat61f( NormalizeAngle(rrEnc.getAngle()));

        System.out.println(s);

    }


    private void enterMaintenanceMode()
    {
            MaintenanceMode.enterMaintenanceMode(driverJoy,
                                                                flEnc,
                                                                frEnc,
                                                                rlEnc,
                                                                rrEnc,
                                                                flDriveMtr,
                                                                frDriveMtr,
                                                                rlDriveMtr,
                                                                rrDriveMtr,
                                                                flTurnMtr,
                                                                frTurnMtr,
                                                                rlTurnMtr,
                                                                rrTurnMtr);
            System.out.println("Exited Maintenance Mode.");
    }


    /**
     * Disable PIDs so we can just turn the wheels
     */
    private void disablePIDs()
    {
        Watchdog.getInstance().feed();
        flPID.disable();
        frPID.disable();
        rlPID.disable();
        rrPID.disable();
    }


    private void enablePIDs()
    {
        Watchdog.getInstance().feed();
        flPID.enable();
        frPID.enable();
        rlPID.enable();
        rrPID.enable();
    }


    private void initMaintenanceMode()
    {
        if(maintenanceModeButtonPressed())
        {
            enterMaintenanceMode();
        }
    }


    public void reenterMaintenanceMode()
    {
        
        disablePIDs();

        enterMaintenanceMode();

        enablePIDs();
    }


    public boolean maintenanceModeButtonPressed()
    {
        if(driverJoy.getRawButton(Utils.MAINTENANCE_MODE_BUTTON_NUM))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    public void pauseDT(boolean pause)
    {
        pauseDT = pause;
    }

    private void driveAuto()
    {
        //Warmup counter counts down to zero and then allows the kicking to
        //begin. Change the value up top to increase/decrease this time.
        if(warmupCounter != 0)
        {
            warmupCounter--;
            return;
        }


        //The number at the end represents the number of clock cycles from the
        //time that the sensor is triggered to the time that the kicker actually
        //kicks. Change this accordingly.
        if((!beamSensor.isTriggered() && !beamSensor.isKicking()) || (beamSensor.isTriggered() && autoKickerCounter !=45))
        {
            goal.Speed = 0.25; //Adjust this speed accordingly
            goal.Angle = 0.0;
            goal.Spin = 0.0;

            if(beamSensor.isTriggered())
            {
                System.out.println("***Incrementing autoKickerCounter");
                autoKickerCounter++;
            }
            System.out.println("Driving forward.");
        }
        else
        {
            System.out.println("******************************STOPPED.");
            goal.Speed = 0.0;
            goal.Angle = 0.0;
            goal.Spin = 0.0;
            autoKickerCounter = 0;

            beamSensor.kickBall(); //uses callback to DefaultRobot to kick ball
        }

    }

    private void driveTele()
    {
        // ----------------------------------------
        // Move All Steering to Angle 0 (Zero)
        // Robot has to be stopped and press both buttons 3 and 4

        if( driverJoy.getRawButton(3) && driverJoy.getRawButton(4)
            && Math.abs(driverJoy.getMagnitude()) < 0.1 && Math.abs(driverJoy.getTwist()) < 0.1 )
        {
            goal.Speed = 0.0;
            goal.Angle = 0.0;
            goal.Spin = 0.0;
            moveSteeringToZero = true;
        }
        else
            moveSteeringToZero = false;


        // ----------------------------------------
        // Get Linear Speed from JoyStick

        double absMagX = Math.abs(driverJoy.getX());
        double absMagY = Math.abs(driverJoy.getY());
        double mag = driverJoy.getMagnitude();
        double absMag = Math.abs(mag);

        if( absMagX > 0.1 || absMagY > 0.1 )
        {
            //goal.Speed = mag;
            goal.Speed = mag/absMag*mag*mag;
            //goal.Speed = mag*mag*mag;
        }
        else
        {
            goal.Speed = 0.0;
        }

        // ----------------------------------------
        // Angle from JoyStick

        double joyAngle = Utils.calculateAng(driverJoy.getX(), -driverJoy.getY()) + 90;
        joyAngleAvg.AddEntry(joyAngle);

        int numAnglesToAvg = (int)( ( 1 - absMag )*49 + 1 );
        goal.Angle = joyAngleAvg.GetAvg(numAnglesToAvg);



        // ----------------------------------------
        // Get Spin from JoyStick

        double twist = driverJoy.getTwist();
        double absTwist = Math.abs(twist);
        double absSpeed = Math.abs(Current.Resultant.Speed);

        if( absTwist > 0.5 )
        {
            goal.Spin = -twist/absTwist*( absTwist - 0.5 ) * 0.5;
            if( absSpeed < 0.75 )
                goal.Spin = goal.Spin * ( 1 - absSpeed );
            else
                goal.Spin = goal.Spin * 0.25;

        }
        else
        {
            goal.Spin = 0;
        }

        if(driverJoy.getRawButton(1))
        {
            goal.Speed = goal.Speed/2;
            System.out.println("Cut speed.");
        }
    }

}

